import processing.serial.*;
int lf = 10;      // ASCII linefeed

// max 360 targets for every simple deg
int[] target_cm = new int[360]; // [0, 359]
int[] target_tmo = new int[360]; // [0, 359]

int scan_deg = 0;
int radius;

// previous x and y length
int prexlen;
int preylen;

// The serial port:
Serial myPort;

void setup() {
  // clear all targets
  for (int i = 0; i < 360; i++) {
    target_tmo[i] = 0;
  }

  size(1900, 1000);
  smooth();

  /* 360 / frame rate: 60 = 6 seconds
   for 1 round of scan rotation
   */
  frameRate(60);

  background(0); // black
  strokeWeight(15);
  ellipseMode(CENTER);

  // radar scan bar radius
  radius = min(width/2, height/2);
  println("Fread Li Sonar radar V1.0. Sep 3, 2023");
  println("scan bar radius = ", radius);

  // List all the available serial ports:
  println("\nfound COM port: ");
  printArray(Serial.list());

  // Open the port you are using at the rate you want:
  myPort = new Serial(this, Serial.list()[0], 115200);
  myPort.bufferUntil(lf);
}

void serialEvent(Serial p) {
  String inString = p.readString();
  int dist_cm = get_cm(inString);
  int angle_deg = get_deg(inString);
  target_cm[angle_deg] = dist_cm;
  target_tmo[angle_deg] = 120; // 120 / frame rate: 60 = 2 seconds
}

void draw() {
  /* draw target
   all targets will be drawn
   within 1 draw() calling
   */
  for (int deg = 0; deg < 360; deg++) {
    if (target_tmo[deg] != 0) {
      target_tmo[deg]--;
      int cm = target_cm[deg];

      /* map target distance cm [4, 150]
       on radar screen:  [50, radius - 7]
       */
      int cm2radius = (int)map(cm, 4, 150, 50, radius - 7);

      // deg -> rad for current angle
      float rad = radians(deg);

      // target color: medium red
      fill(#F70A1E);
      stroke(#F70A1E);

      // draw a target
      ellipse(width/2 + cm2radius * cos(rad), height/2 + cm2radius * sin(rad), 5, 5);
    }
  }

  // radar scan bar
  float scan_rad = radians(scan_deg);
  int xlen = (int)(radius * cos(scan_rad));
  int ylen = (int)(radius * sin(scan_rad));

  /* radar scan
   1 draw() calling only draw
   1 dark and 1 light scan bar
   so, 1 circle(360 deg) scan need 360 times of calling draw()
   */
  stroke(#646420); // radar scan background. dark green
  line(width/2, height/2, width/2 + prexlen, height/2 + preylen);

  stroke(#EFF00F); // radar scan active bar. light green
  line(width/2, height/2, width/2 + xlen, height/2 + ylen);

  if (++scan_deg > 360) {
    scan_deg = 0;
  }
  prexlen = xlen;
  preylen = ylen;
}

/*
for simply debug, I make get_cm and get_deg
 individually, in future optimize, they should be
 combined into 1 routine. Eg: ret_type get_cm_deg(String str)
 ret_typ is a struct of cm & deg, or a class var etc...
 */
int get_cm(String str) {
  // 01234
  // 123C
  String tmpStr = "";
  for (int i = 0; i <= 3; i++) { // [2, 200]
    if (str.charAt(i) == 'C') { // Eg: 14C309D means 14CM
      break;
    }
    tmpStr = tmpStr +str.charAt(i);
  }

  return int(tmpStr);
}

int get_deg(String str) {
  // skip "C"
  // 01234
  // 123C
  int i = 0;
  for (; i <=3; i++) { // [2, 200]
    if (str.charAt(i) == 'C') { // Eg: 14C309D means 14CM
      i++;
      break;
    }
  }

  String tmpStr = "";
  // 01234567
  // 123C359D  --> 123cm  359 deg
  for (; i <= 7; i++) { // [0, 359]
    if (str.charAt(i) == 'D') { // Eg: 14C309D means 309deg
      break;
    }
    tmpStr = tmpStr +str.charAt(i);
  }
  return int(tmpStr);
}
